
public class Agressive extends Proportional {
	
	public Agressive() {
		// initializing with 1 to prevent division with 0
		m_highestVal = 1;
		setWaitTime(5);
	}

	public void doWork() throws Exception {
		
		//initializing controller matrix for agressive behaviour
		int[][] k = new int[][] {{0,1,0,1,0,1},{1,0,1,0,1,0}};

		//getting sensor values
		int s0 = getLightValue(0);
		int s1 = getLightValue(1);
		int s2 = getLightValue(2);
		int s3 = getLightValue(3);
		int s4 = getLightValue(4);
		int s5 = getLightValue(5);
		
		//getting rough values for left and right motor speed
		int aL = s0*k[0][0] + s1*k[0][1] + s2*k[0][2] + s3*k[0][3] + s4*k[0][4] + s5*k[0][5];
		int aR = s0*k[1][0] + s1*k[1][1] + s2*k[1][2] + s3*k[1][3] + s4*k[1][4] + s5*k[1][5];
		
		//Searching for biggest speed value
		if (aL > m_highestVal) {
			m_highestVal = aL;
		}
		if (aR > m_highestVal) {
			m_highestVal = aR;
		}
		
		//scaling the speed value between 0 and 5
		int speedL = (5 * aL) / m_highestVal;
		int speedR = (5 * aR) / m_highestVal;
		
		this.setMotorSpeeds(speedL, speedR);
		
		//if no lightbulb is in reach (high sensor value) avoid wall contact
		if ((aL + aR)/2 > 1300) {
			handleWallContact();
		}
		
		System.out.println("left: " + getLeftWheelPosition() + "  " + "right: " + getRightWheelPosition());

	}


	public void close() throws Exception {
	}

}

